﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace SpatialHashing
{
    class SpatialHash<T> where T : IElement
    {
        private float cellSize;
        private float cellDiagonal;
        private Dictionary<Vector3, SpatialCell<T>> cells;

        public SpatialHash(float cellSize)
        {
            this.cellSize = cellSize;
            this.cellDiagonal = (float)Math.Sqrt(3 * (cellSize * cellSize));
            cells = new Dictionary<Vector3, SpatialCell<T>>();
        }

        public void add(T item)
        {
            //generate center from position
            Vector3 center = generateCellCenter(item.getPosition());

            //check if a cell with that center already exists
            SpatialCell<T> sc;
            if (cells.TryGetValue(center, out sc))
            {
                //if so, SpatialCell at that location already exists, so just add item to it
                sc.addItem(item);
            }
            else
            {
                //if not, create SpatialCell with item and add it to dictionary
                sc = new SpatialCell<T>(center, cellSize);
                sc.addItem(item);
                cells.Add(center, sc);
            }
        }
        public float getCellSize()
        {
            return cellSize;
        }
        public float getCellDiagonal()
        {
            return cellDiagonal;
        }

        private Vector3 generateCellCenter(Vector3 position)
        {
            float x = (float)Math.Floor((double)(position.x / cellSize)) * cellSize;
            float y = (float)Math.Floor((double)(position.y / cellSize)) * cellSize;
            float z = (float)Math.Floor((double)(position.z / cellSize)) * cellSize;

            float delta = 0.5f * cellSize;
            Vector3 center = new Vector3(x + delta, y + delta, z + delta);

            return center;
        }
        public bool getCellItemsRef(Vector3 position, out LinkedList<T> itemsRef)
        {
            Vector3 center = generateCellCenter(position);

            SpatialCell<T> sc;
            if (cells.TryGetValue(center, out sc))
            {
                itemsRef = sc.getItemsRef();
                return true;
            }

            itemsRef = null;
            return false;
        }

        private List<Vector3> getCellCentersOfDistantCells(Vector3 position, int cellDistance)
        {
            //calculate center of nearest bottomLeft cell of the layer we're interested in
            Vector3 centerCenter = generateCellCenter(position);
            Vector3 originCenter = centerCenter;
            originCenter.x -= (cellDistance * cellSize);
            originCenter.y -= (cellDistance * cellSize);
            originCenter.z -= (cellDistance * cellSize);

            //loop over all cells in cube, leaving out those not the edge
            int min = 0;
            int max = (2 * cellDistance);
            List<Vector3> cellCenters = new List<Vector3>();

            for (int i = min; i <= max; i++)
            {
                for (int j = min; j <= max; j++)
                {
                    for (int k = min; k <= max; k++)
                    {
                        if (i == min || i == max || j == min || j == max || k == min || k == max)
                        {
                            Vector3 c = new Vector3(originCenter.x + (i * cellSize), originCenter.y + (j * cellSize), originCenter.z + (k * cellSize));
                            Vector3 center = generateCellCenter(c);

                            if (cells.ContainsKey(center))
                            {
                                cellCenters.Add(center);
                            }
                        }
                    }
                }
            }
            return cellCenters;
        }
        public bool findNearestElement(Vector3 position, float minDistance, float maxDistance, out float nearestDistance, out T nearestElement)
        {
            float minDistSq = minDistance * minDistance;
            float maxDistSq = maxDistance * maxDistance;

            int minCellDistance = (int)(minDistance / cellDiagonal);
            int maxCellDistance = (int)(maxDistance / cellSize) + 1;

            int cellDistance = minCellDistance;
            while (cellDistance <= maxCellDistance)
            {
                //get the cell centers of all relevant cells
                List<Vector3> cellCenters = getCellCentersOfDistantCells(position, cellDistance);

                //loop over these cells
                T bestElement = default(T);
                float bestDistSq = float.MaxValue;

                for (int i = 0; i < cellCenters.Count; i++)
                {
                    //get items in cell
                    LinkedList<T> items;
                    getCellItemsRef(cellCenters[i], out items);

                    //loop over all items in cell
                    for (LinkedListNode<T> it = items.First; it != null; it = it.Next)
                    {
                        Vector3 p = it.Value.getPosition();

                        float dX = (position.x - p.x);
                        float dY = (position.y - p.y);
                        float dZ = (position.z - p.z);
                        float distSq = (dX * dX) + (dY * dY) + (dZ * dZ);

                        if (minDistSq <= distSq && distSq <= maxDistSq)
                        {
                            if (distSq < bestDistSq)
                            {
                                bestDistSq = distSq;
                                bestElement = it.Value;
                            }
                        }
                    }
                }

                //we're done if we found an element in the layer
                if (bestDistSq != float.MaxValue)
                {
                    nearestDistance = (float)Math.Sqrt((float)bestDistSq);
                    nearestElement = bestElement;
                    return true;
                }

                cellDistance++;
            }

            //if we're here, then we didn't find a single element
            nearestDistance = float.MaxValue;
            nearestElement = default(T);
            return false;
        }
    }
}
